#include "PathSearch.h"

namespace fullsail_ai { namespace algorithms {

	PlannerNode::PlannerNode(Tile* _state,PlannerNode * _parent,double _heuristicCost,double _givenCost)
	{
		state = _state;
		parent = _parent;
		heuristicCost = _heuristicCost;
		givenCost = _givenCost;
	}
	PathSearch::PathSearch() : open(&fullsail_ai::algorithms::compare)
	{
		tilemap = NULL;
		StartTile = NULL;
		GoalTile = NULL;
		
	}

	PathSearch::~PathSearch()
	{
	}

	void PathSearch::initialize(TileMap* _tileMap)
	{
		tilemap = _tileMap;
		m_isDone = false;
	}

	void PathSearch::enter(int startRow, int startColumn, int goalRow, int goalColumn)
	{
		if(startRow >= 0  && startRow < tilemap->getRowCount())
			StartTile = tilemap->getTile(startRow,startColumn);
		if(goalRow >=0 && goalColumn < tilemap->getColumnCount())
			GoalTile = tilemap->getTile(goalRow,goalColumn);
		//Maybe?
		open.push(new PlannerNode(StartTile,NULL,dist(StartTile,GoalTile)*1.1,0));
		m_Dups[StartTile] = open.front();
		m_isDone = false;
		curPlanNode = NULL;
	}

	void PathSearch::update(long timeslice)
	{
		PlannerNode *planNode = NULL;
		double GCost;
		//vector<Tile*> successor;
		while(!open.empty())
		{
			curPlanNode = open.front();
			open.pop();
			if(curPlanNode->state == GoalTile)
			{
				cout << "Give Cost" << curPlanNode->givenCost << "\n\n";
				m_isDone = true;
				while(curPlanNode != NULL)
				{
					PathChosen.push_back(curPlanNode->state);
					//curPlanNode->state->setLineTo(curPlanNode->parent->state,0xFF00FF00);
					curPlanNode = curPlanNode->parent;
				}
				cout << "Soultion Size: "<<PathChosen.size()<<"\n\n";
				break;
			}
			areAdjacent(curPlanNode);
			for(size_t i = 0; i < CloseNodes.size();i++)
			{
				GCost = curPlanNode->givenCost + (dist(CloseNodes[i],curPlanNode->state) * CloseNodes[i]->getWeight());/*GivenCost + successorDistance * weight*/ 
				planNode = m_Dups[CloseNodes[i]];
				if(planNode)
				{
						if(planNode->givenCost > GCost)
						{
							open.remove(planNode);
							planNode->givenCost = GCost;
							// Maybe?
							planNode->heuristicCost = dist(planNode->state,GoalTile)*1.1;
							planNode->parent = curPlanNode;
							open.push(planNode);
							CloseNodes[i]->setFill(0xFFFF0000);
						}
				}
				else
				{
					PlannerNode* succNode = new PlannerNode(CloseNodes[i],curPlanNode,dist(CloseNodes[i],GoalTile)*1.1,GCost);
					m_Dups[succNode->state] = succNode;
					open.push(succNode);
					CloseNodes[i]->setFill(0xFF0000FF);
				}
			}
// 			PlannerNode *LineDraw = curPlanNode;
// 			while(LineDraw->parent != NULL)
// 			{
// 				LineDraw->state->setLineTo(LineDraw->parent->state,0xFFFF0000);
// 				LineDraw = LineDraw->parent;
// 			}
			if(timeslice == 0)
				break;
		}
	}

	void PathSearch::exit()
	{
		map<Tile const*,PlannerNode*>::iterator i = m_Dups.begin();
		for(;i != m_Dups.end();i++ )
		{	
			delete i->second;
		}
		open.clear();
		m_Dups.clear();
		PathChosen.clear();
	}

	void PathSearch::shutdown()
	{
		exit();
	}

	bool PathSearch::isDone() const
	{
		return m_isDone;
	}

	std::vector<Tile const*> const PathSearch::getSolution() const
	{
		//std::vector<Tile const*> temp;
		return PathChosen;
	}

	void PathSearch::areAdjacent( PlannerNode* _curPlanNode )
	{
		CloseNodes.clear();
		Tile* CurNode = _curPlanNode->state;
		//CloseNodes.push_back(tilemap->getTile(CurNode->getRow(),CurNode->getColumn()));
		Tile*curTile = tilemap->getTile(CurNode->getRow()-1,CurNode->getColumn());
		if(curTile != NULL && curTile->getWeight() != 0)
			CloseNodes.push_back(curTile);

		curTile = tilemap->getTile(CurNode->getRow()+1,CurNode->getColumn());
		if(curTile != NULL && curTile->getWeight() != 0)
			CloseNodes.push_back(curTile);

		curTile = tilemap->getTile(CurNode->getRow(),CurNode->getColumn()-1);
		if(curTile != NULL && curTile->getWeight() != 0)
			CloseNodes.push_back(curTile);

		curTile = tilemap->getTile(CurNode->getRow(),CurNode->getColumn()+1);
		if(curTile != NULL && curTile->getWeight() != 0)
			CloseNodes.push_back(curTile);
		if((CurNode->getRow()%2) == 0)
		{	
			// Even
			curTile = tilemap->getTile(CurNode->getRow()-1,CurNode->getColumn()-1);
			if(curTile != NULL && curTile->getWeight() != 0)
				CloseNodes.push_back(curTile);
			curTile = tilemap->getTile(CurNode->getRow()+1,CurNode->getColumn()-1);
			if(curTile != NULL && curTile->getWeight() != 0)
				CloseNodes.push_back(curTile);
		}
		else
		{
			//Odd
			curTile = tilemap->getTile(CurNode->getRow()-1,CurNode->getColumn()+1);
			if(curTile != NULL && curTile->getWeight() != 0)
				CloseNodes.push_back(curTile);
			curTile = tilemap->getTile(CurNode->getRow()+1,CurNode->getColumn()+1);
			if(curTile != NULL && curTile->getWeight() != 0)
				CloseNodes.push_back(curTile);
		}
// 		for(size_t i = 0; i < CloseNodes.size();i++)
// 		{
// 			if(CloseNodes[i] == NULL||CloseNodes[i]->getWeight() == 0)
// 			{
// 				CloseNodes.erase(CloseNodes.begin()+i);
// 				--i;
// 			}
// 		}
		//return CloseNodes;
	}

	double PathSearch::dist( Tile const* _tile1,Tile const* _tile2 )
	{
		double ret =((_tile2->getXCoordinate() - _tile1->getXCoordinate())*(_tile2->getXCoordinate() - _tile1->getXCoordinate()))+
			((_tile2->getYCoordinate() - _tile1->getYCoordinate())* (_tile2->getYCoordinate() - _tile1->getYCoordinate()));
		ret = sqrt(ret);
		return ret;
	}

	bool compare( PlannerNode*const& l, PlannerNode*const& h )
	{
		return ((l->givenCost+l->heuristicCost) > (h->givenCost+h->heuristicCost));
	}

}}  // namespace fullsail_ai::algorithms

